#ifndef CC_DECODE_h
#define CC_DECODE_h
#include <math.h>

#include <iostream>

#define pi 3.1415926
#define DATASIZE 20

int nms_cpu(float const* Box, float const* score, bool* keep, int kInfoNumber,
            float const& iou_threshold, int filled_size);

template <typename T>
inline float get(T origin, std::uint32_t scale) {
  return float(origin) / float(pow(2, scale));
}

// int decode_chw_center(AmbaNode const* out_cls_node, AmbaNode const*
// out_xy_node,
//                       AmbaNode const* out_z_node, AmbaNode const*
//                       out_lwh_node, AmbaNode const* out_rot_node, AmbaNode
//                       const* out_vel_node, float const* ClassThreshold,
//                       float* scores, float* Bbox, int* labels,
//                       int const kMaxResultSize, int filled_size_,
//                       int const NetLength, int const NetWidth,
//                       int const ClassNumber, int class_base, int kInfoNumber,
//                       const int downsize, float const* voxels_size,
//                       float const* pts_range)
template <typename T>
int decode_chw_center(
    char const* out_cls_node, char const* out_xy_node, char const* out_z_node,
    char const* out_lwh_node, char const* out_rot_node,
    char const* out_vel_node, std::int32_t const* out_cls_node_para,
    std::int32_t const* out_xy_node_para, std::int32_t const* out_z_node_para,
    std::int32_t const* out_lwh_node_para,
    std::int32_t const* out_rot_node_para,
    std::int32_t const* out_vel_node_para, float const* ClassThreshold,
    float* scores, float* Bbox, int* labels, float* total_scores,
    int const kMaxResultSize, int filled_size_, int const NetLength,
    int const NetWidth, int const ClassNumber, int class_base, int kInfoNumber,
    const int downsize, float const* voxels_size, float const* pts_range,
    std::int8_t* scales) {
  T* out_cls = (T*)out_cls_node;
  T* out_xy = (T*)out_xy_node;
  T* out_z = (T*)out_z_node;
  T* out_lwh = (T*)out_lwh_node;
  T* out_rot = (T*)out_rot_node;
  T* out_vel = (T*)out_vel_node;

  /*    CHW support
   *  float support
   */
  float x_stride = voxels_size[0] * downsize;
  float y_stride = voxels_size[1] * downsize;

  // float x_offset = x_stride + pts_range[0];
  // float y_offset = y_stride + pts_range[1];
  float x_offset = pts_range[0];
  float y_offset = pts_range[1];
  float class_threshold[DATASIZE]{0};

  for (std::uint16_t i{0U}; i < ClassNumber; ++i) {
    class_threshold[i] = -1 * log((1 - ClassThreshold[i]) / ClassThreshold[i]);
  }

  std::uint32_t xy_step{static_cast<std::uint32_t>(NetLength * NetWidth)};
  std::uint32_t xy_step_box{
      static_cast<std::uint32_t>(NetLength * NetWidth * kInfoNumber)};
  std::uint32_t xy_step_dir{
      static_cast<std::uint32_t>(NetLength * NetWidth * 1)};

  float max_score{0};
  std::int8_t label;
  for (std::uint16_t i{0U}; i < out_cls_node_para[3]; ++i) {    // NetLength
    for (std::uint16_t j{0U}; j < out_cls_node_para[2]; ++j) {  // NetWidth
      std::uint64_t index_{static_cast<std::uint64_t>(j * NetLength + i)};
      for (std::uint16_t k{0U}; k < ClassNumber; ++k) {
        // step1: decode score and label
        std::uint64_t cr_index{index_ + (k * 1) * xy_step};
        max_score = get<T>(out_cls[cr_index], scales[0]);

        label = k;

        if (max_score < class_threshold[label]) {
          continue;
        }

        // step2: decode box
        // std::cout << "---------------------------- decode "
        //              "bbox------------------------------"
        //           << std::endl;
        if (filled_size_ >= kMaxResultSize) {
          return filled_size_;
        }
        //          cr_index = index_ + (k * 1 + l) * xy_step_box;
        cr_index = index_;

        // 这里的kInfoNumber 由7改为9
        //   scores[filled_size_] = sigmoid(max_score);
        scores[filled_size_] = 1 / (1 + exp(-max_score));
        labels[filled_size_] = label + class_base;
        // Bbox[filled_size_ * kInfoNumber + 0] =
        //     (get<T>(out_xy[cr_index + 0 * xy_step], scales[1])) * x_stride
        //     + j * x_stride + x_offset;
        // Bbox[filled_size_ * kInfoNumber + 1] =
        //     (get<T>(out_xy[cr_index + 1 * xy_step], scales[1])) * y_stride
        //     + i * y_stride + y_offset;
        Bbox[filled_size_ * kInfoNumber + 0] =
            (get<T>(out_xy[cr_index + 0 * xy_step], scales[1])) * x_stride +
            i * x_stride + x_offset;
        Bbox[filled_size_ * kInfoNumber + 1] =
            (get<T>(out_xy[cr_index + 1 * xy_step], scales[1])) * y_stride +
            j * y_stride + y_offset;
        Bbox[filled_size_ * kInfoNumber + 2] =
            get<T>(out_z[cr_index], scales[2]);
        Bbox[filled_size_ * kInfoNumber + 3] =
            exp(get<T>(out_lwh[cr_index + 0 * xy_step], scales[3]));
        Bbox[filled_size_ * kInfoNumber + 4] =
            exp(get<T>(out_lwh[cr_index + 1 * xy_step], scales[3]));
        Bbox[filled_size_ * kInfoNumber + 5] =
            exp(get<T>(out_lwh[cr_index + 2 * xy_step], scales[3]));
        Bbox[filled_size_ * kInfoNumber + 6] =
            atan2(get<T>(out_rot[cr_index + 0 * xy_step], scales[4]),
                  get<T>(out_rot[cr_index + 1 * xy_step], scales[4]));
        Bbox[filled_size_ * kInfoNumber + 7] =
            (get<T>(out_vel[cr_index + 0 * xy_step], scales[5]));
        Bbox[filled_size_ * kInfoNumber + 8] =
            (get<T>(out_vel[cr_index + 1 * xy_step], scales[5]));
        for (int ii = 0; ii < ClassNumber; ++ii) {
          std::uint64_t cr_index{index_ + (ii * 1) * xy_step};
          float temp = get<T>(out_cls[cr_index], scales[0]);
          float score = 1 / (1 + exp(-temp));
          total_scores[filled_size_ * ClassNumber + ii] = score;
        }

        // std::cout << " score = " << scores[filled_size_]
        //           << ", label = " << labels[filled_size_]
        //           << " , x = " << Bbox[filled_size_ * kInfoNumber + 0]
        //           << " , y = " << Bbox[filled_size_ * kInfoNumber + 1]
        //           << ", z = " << Bbox[filled_size_ * kInfoNumber + 2]
        //           << ", lwh = " << Bbox[filled_size_ * kInfoNumber + 3]
        //           << " "
        //           << Bbox[filled_size_ * kInfoNumber + 4] << " "
        //           << Bbox[filled_size_ * kInfoNumber + 5]
        //           << " , rot = " << Bbox[filled_size_ * kInfoNumber +
        //           6]
        //           << " , vel = " << Bbox[filled_size_ * kInfoNumber +
        //           7]
        //           << " " << Bbox[filled_size_ * kInfoNumber + 8] <<
        //           std::endl;
        filled_size_++;
      }
    }
  }
  return filled_size_;
}

#endif
